#include "robotException.h"


RobotException::RobotException(): location_recv_(false), stopLocation_(false), 
                                   location_nodata(false), locationdrift_(false),
                                   exceptionType_(NOEXCEPTION)   
{
    ros::NodeHandle privateNh("~");
    //话题参数
    privateNh.param("systemStatusTopic", systemStatusTopic_, std::string("/system_event"));
    privateNh.param("frontLaserTopic", frontLaserTopic_, std::string("/front_scan"));
    privateNh.param("backLaserTopic", backLaserTopic_, std::string("/back_scan"));
    privateNh.param("rightLaserTopic", rightLaserTopic_, std::string("/left_urg_node/cloud_left_360"));
    privateNh.param("odomTopic", odomTopic_, std::string("/odom"));
    privateNh.param("imuTopic", imuTopic_, std::string("/imu_msgs"));
    privateNh.param("frontCameraTopic", frontCameraTopic_, std::string("/front_camera/front_camera/depth/camera_info"));
    privateNh.param("backCameraTopic", backCameraTopic_, std::string("/back_camera/back_camera/depth/camera_info"));
    privateNh.param("locationSoftTopic", locationSoftTopic_, std::string("/bridge_node/client_control_mode"));
    privateNh.param("locationPoseTopic", locationPoseTopic_, std::string("/bridge_node/client_localization_pose/pose"));
    privateNh.param("locationDriftTopic", locationDriftTopic_, std::string("/location_drift"));
    privateNh.param("baseInfoTopic", baseInfoTopic_, std::string("/base_info"));
    privateNh.param("mcuTimeOutTopic", mcuTimeOutTopic_, std::string("/timeOuterr"));
    privateNh.param("mcuErrorTopic", mcuErrorTopic_, std::string("/mcuerror"));
    //检测开关
    privateNh.param("checkSystemStatus", checkSystemStatus_, true);
    privateNh.param("checkFrontLaser", checkFrontLaser_, true);
    privateNh.param("checkBackLaser", checkBackLaser_, true);
    privateNh.param("checkRightLaser", checkRightLaser_, true);
    privateNh.param("checkFrontCamera", checkFrontCamera_, true);
    privateNh.param("checkBackCamera", checkBackCamera_, true);
    privateNh.param("checkOdom", checkOdom_, true);
    privateNh.param("checkImu", checkImu_, true);
    privateNh.param("checkLocationSoft", checkLocationSoft_, true);
    privateNh.param("checkLocationPose", checkLocationPose_, true);
    privateNh.param("checkLocationDrift", checkLocationDrift_, true);
    privateNh.param("checkBaseInfo", checkBaseInfo_, true);
    privateNh.param("checkMcuTimeOut", checkMcuTimeOut_, true);
    privateNh.param("checkMcuError", checkMcuError_, true);
    //检测导航节点是否存在，如果不存在，报相应的节点error
    privateNh.param("frontLaserNode", frontLaserNode_, std::string("/rslidar_sdk_node"));
    privateNh.param("backLaserNode", backLaserNode_, std::string("/wj_716N_lidar_back"));
    privateNh.param("rightLaserNode", rightLaserNode_, std::string("/left_urg_node"));
    privateNh.param("frontCameraNode", frontCameraNode_, std::string("/front_camera/front_camera"));
    privateNh.param("backCameraNode", backCameraNode_, std::string("/back_camera/back_camera"));
    privateNh.param("autoChargeNode", autoChargeNode_, std::string("/auto_charge_node"));
    privateNh.param("odomNode", odomNode_, std::string("/odom_node"));
    privateNh.param("ImuNode", ImuNode_, std::string("/imu_atom_c2_node"));
    privateNh.param("interfaceNode", interfaceNode_, std::string("/interface_node"));
    privateNh.param("systemNode", systemNode_, std::string("/system_controller_node"));
    privateNh.param("navigationNode", navigationNode_, std::string("/kybot_target_pose_navigation"));
    privateNh.param("perceptionNode", perceptionNode_, std::string("/kybot_perception_node"));
    privateNh.param("mapNode", mapNode_, std::string("/map_manager_node"));
    privateNh.param("accuratePositionNode", accuratePositionNode_, std::string("/accurate_positioning_Node"));
    privateNh.param("writeBagNode", writeBagNode_, std::string("/WriteBag"));
    privateNh.param("locationSoftNode", locationSoftNode_, std::string("/bridge_node"));
    privateNh.param("locationSoftServerNode", locationSoftServerNode_, std::string("/server_bridge_node"));
    privateNh.param("mcuNode", mcuNode_, std::string("/mcu_node"));
    privateNh.param("joyNode", joyNode_, std::string("/joy_node"));
    privateNh.param("joyControlNode", joyControlNode_, std::string("/joy_control"));
    privateNh.param("autoDeleteNode", autoDeleteNode_, std::string("/AutoDeleteNode"));
      // 初始化 glog
    std::string username = std::getenv("USER");                              // 获取当前用户名
    std::string logdir = "/home/" + username + "/logdir/kybot_exception/";  // 日志目录路径
    InitGlog(logdir, "kybot_exception");

}

// glog配置
void RobotException::InitGlog(const std::string& log_dir, const std::string& log_name) {
    google::InitGoogleLogging(log_name.c_str());
    google::InstallFailureSignalHandler();
    // 设置 glog 参数
    FLAGS_logbufsecs = 0;                    // 日志实时输出
    FLAGS_max_log_size = 10;                 // 最大日志文件大小 10M
    FLAGS_logtostderr = false;               // 日志消息不输出到标准错误
    FLAGS_stop_logging_if_full_disk = true;  // 磁盘满时停止日志记录
    FLAGS_colorlogtostderr = true;           // 设置记录到标准输出的颜色消息
    FLAGS_alsologtostderr = true;            // 日志消息同时输出到标准错误

    // 检查日志目录是否存在，如果不存在则创建
    if (access(log_dir.c_str(), 0) == -1) {
        std::string command = "mkdir -p " + log_dir;
        int result = system(command.c_str());
        if (result != 0) {
        LOG(ERROR) << "Failed to create log directory: " << log_dir;
        }
    }

    // 设置日志文件输出路径
    google::SetLogDestination(google::INFO, (log_dir + log_name).c_str());
    LOG(INFO) << "Glog ON";
}

void RobotException::initialize()
{
    
    if(checkSystemStatus_)
    navigationResultSuber_ = nh_.subscribe(systemStatusTopic_, 1, &RobotException::navigationResultCallback, this);

    //硬件检测订阅器
    if(checkFrontCamera_)
    {
        frontcameraSuber_ = nh_.subscribe<sensor_msgs::CameraInfo>(frontCameraTopic_, 1,
                            boost::bind(&RobotException::HardwareCallback<const sensor_msgs::CameraInfo::ConstPtr&>, this, _1, &hardware_check_map_["frontCameraError"]));
        hardware_check_map_["frontCameraError"] = true;
    }
    if(checkBackCamera_)
    {
        backcameraSuber_ = nh_.subscribe<sensor_msgs::CameraInfo>(backCameraTopic_, 1,
                            boost::bind(&RobotException::HardwareCallback<const sensor_msgs::CameraInfo::ConstPtr&>, this, _1, &hardware_check_map_["backCameraError"]));
        hardware_check_map_["backCameraError"] = true;
    }
    if(checkFrontLaser_)
    {
        frontlaserSuber_ = nh_.subscribe<sensor_msgs::LaserScan>(frontLaserTopic_, 1,
                            boost::bind(&RobotException::HardwareCallback<const sensor_msgs::LaserScan::ConstPtr&>, this, _1, &hardware_check_map_["frontLaserError"]));
        hardware_check_map_["frontLaserError"] = true;
    }
    if(checkBackLaser_)
    {
        backlaserSuber_ = nh_.subscribe<sensor_msgs::LaserScan>(backLaserTopic_, 1,
                            boost::bind(&RobotException::HardwareCallback<const sensor_msgs::LaserScan::ConstPtr&>, this, _1, &hardware_check_map_["backLaserError"]));
        hardware_check_map_["backLaserError"] = true;
    }
    if(checkRightLaser_)
    {
        rightlaserSuber_ = nh_.subscribe<sensor_msgs::PointCloud2>(rightLaserTopic_, 1,
                            boost::bind(&RobotException::HardwareCallback<const sensor_msgs::PointCloud2::ConstPtr&>, this, _1, &hardware_check_map_["rightLaserError"]));
        hardware_check_map_["rightLaserError"] = true;
    }
    if(checkOdom_)
    {
        odom_suber_ = nh_.subscribe<nav_msgs::Odometry>(odomTopic_, 1,
                            boost::bind(&RobotException::HardwareCallback<const nav_msgs::Odometry::ConstPtr&>, this, _1, &hardware_check_map_["odomError"]));
        hardware_check_map_["odomError"] = true;
    }
    if(checkImu_)
    {
        imu_suber_ = nh_.subscribe<sensor_msgs::Imu>(imuTopic_, 1,
                            boost::bind(&RobotException::HardwareCallback<const sensor_msgs::ImuConstPtr&>, this, _1, &hardware_check_map_["imuError"]));
        hardware_check_map_["imuError"] = true;
    }
    // 其他订阅器
    exception_taskPuber_ =  nh_.advertise<kybot_msg::Exceptiontonavigation>("/exception_task", 1);
    if(checkLocationSoft_)
    Location_soft_ = nh_.subscribe<bosch_locator_bridge::ClientControlMode>(locationSoftTopic_, 1, &RobotException::locationsoftCallback, this);
    if(checkLocationPose_)
    Location_pose_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>(locationPoseTopic_,1,&RobotException::locationposeCallback, this);
    exceptionNotifySuber_ = nh_.subscribe("/exceptionNotify", 20, &RobotException::exceptionNotifyCallback, this);
    exceptionTypePuber_ = nh_.advertise<std_msgs::Int8>("/exceptionType", 1);
    if(checkLocationDrift_)
    Location_driftSuber_ = nh_.subscribe<std_msgs::Bool>(locationDriftTopic_,1, &RobotException::locationdriftCallback, this);
    if(checkBaseInfo_)
    baseinfo_sub_ = nh_.subscribe("/base_info", 1, &RobotException::BaseInfoCallback, this);
    if(checkMcuTimeOut_)
    mcusend_sub_  = nh_.subscribe("/timeOuterr", 1, &RobotException::McusendCallback, this);
    if(checkMcuError_)
    mcuerror_sub_ = nh_.subscribe("/mcuerror", 1, &RobotException::McuerrorCallback, this);

    // 服务初始化
    ros::NodeHandle privateNh1("~");
    exceptionNotifySrv_ = privateNh1.advertiseService("exception1_notify_srv", &RobotException::exceptionNotifySrvCallback, this);
    stop_location_srv_ = privateNh1.advertiseService("stopLocationSrv", &RobotException::stopLocationSrvcallback, this);

    // 读取异常列表
    readExceptionList();
}

void RobotException::run()
{
    // todo: 机器人异常报警主程序
    ros::Duration(6).sleep(); // 延时5s开始检测
    ros::Rate loop_rate(4);
    std_msgs::Int8 _type;
    int count = 0;
    kybot_msg::Exceptiontonavigation exceptiontask;
    int check_count = 0;
    while (ros::ok())
    {
        
        ros::spinOnce();
        if (!exceptionList_.empty())
        {
            count ++;
            check_count =0;
            if(count < 3)
            {
               exceptionList_.clear(); 
            }
            else
            {
                exceptiontask.navigation_result = "";
                if(exceptionList_.size() == 1 && exceptionList_.front() == "find_obs")
                {
                    exceptiontask.pausetask = false;
                    exceptiontask.stoptask = false;
                    exceptiontask.navigation_result = "";
                }
                else
                {
                    exceptiontask.pausetask = true;
                    exceptiontask.stoptask = false;
                    for (auto error : exceptionList_)
                    {
                      LOG(INFO) << "error: " << error.c_str();
                      exceptiontask.navigation_result += error + ";";
                    }
                }
                lastList_.clear();
                lastList_.assign(exceptionList_.begin(), exceptionList_.end());
                count = 0; 
            }      
        }
        else
        {
            count = 0;
            check_count ++;
            if(check_count ==3)
            {
                exceptiontask.pausetask = false;
                exceptiontask.stoptask = false;
                exceptiontask.navigation_result = "";
                check_count = 0;
               lastList_.clear();
               lastList_.assign(exceptionList_.begin(), exceptionList_.end());
            }
        }
       
        // 发布当前异常状态
        _type.data = getExceptionType();
        exceptionTypePuber_.publish(_type);
        if(std::find(mcucheckList_.begin(), mcucheckList_.end(), "Emergencystop_timeout") != mcucheckList_.end())
        {
            exceptiontask.pausetask = false;
            exceptiontask.stoptask = true;
            exceptiontask.navigation_result = "Emergencystop_timeout";
        }

        // 发布异常任务
        exception_taskPuber_.publish(exceptiontask);

        // 处理位置漂移
        if(locationdrift_)
        {
            LOG(INFO) << "location was drift ";
            lastList_.push_back("location was drift");
            locationdrift_ = false;
        }

        // 特殊处理MCU超时
        if(mcucheckList_.size()>0)
        {
            for(int i=0;i<mcucheckList_.size();i++)
            {
                lastList_.push_back(mcucheckList_[i]);
                LOG(INFO) << "mcucheckList_" <<mcucheckList_[i].c_str();
            }
        }
        mcucheckList_.clear();

         // 执行检测
        HardwareCheck();
        nodecheck();
        locationcheck();

        // 位置数据检查
        if(location_nodata == true && !stopLocation_)
        {
            AddErrorToErrorlist("locationpose_nodata");
        }

        // 重置状态
        location_nodata = false;
        location_recv_ = false;

        //stopLocation_ = false;
        loop_rate.sleep();
    }
   
}
bool RobotException::readExceptionList()
{
    std::string exceptionListFilePath = ros::package::getPath("kybot_exception") + "/data/exceptionList.json";

    if (!boost::filesystem::exists(exceptionListFilePath))
    {
        LOG(INFO) << "File does not exists" << exceptionListFilePath.data();
        return false;
    }
    
    
    // JsonParser object;
    // std::ifstream file;
    // file.open(exceptionListFilePath);
    // 1. 读取 JSON 文件
    std::ifstream file1(exceptionListFilePath);
    try
    {
        
        if (!file1.is_open()) {
                LOG(INFO)<<"无法打开文件 employees.json";
            }
        
        //2 . 解析JSON 文件
        data = json::parse(file1);
        // file >> object;
    }
    catch (...)
    {
        LOG(INFO)<< "JSON file format invalid!" ;
        return false;
    }
    // file.close();
    try
    {
        auto normalList = data["warning"];
        auto fatalList = data["fatal"];
        int normalSize = normalList.size();
        int fatalSize = fatalList.size();

        for (int i = 0; i < normalSize; i++)
        {
            normalList_.push_back(normalList[i]);
            std::cout << normalList[i] << std::endl;
        }
        for (int i = 0; i < fatalSize; i++)
        {
            fatalList_.push_back(fatalList[i]);
            std::cout << fatalList[i] << std::endl;
        }
    }
    catch (...)
    {
        LOG(INFO)<<"Parse JSON error!";
        return false;
    }
    return true;
}

/*** 
        1.当前的异常列表里查是否有fatalList中的内容，如果有一个，则状态为FATAL;
        2.否则在异常列表里查是否有normalList中的内容，如果有一个，则状态为NORMAL;
        3.都没有则为NOEXCEPTION
***/
int  RobotException::getExceptionType()
{
    if (exceptionList_.empty())
    {
        return exceptionType(0) ;
    }


    int exceptionNum = exceptionList_.size();
    for (int i = 0; i < exceptionNum; i++)
    {
        // 检查致命错误
        if (std::find(fatalList_.begin(), fatalList_.end(), exceptionList_[i]) != fatalList_.end())
        {
            return exceptionType(3);
        }

        // 检查警告
        for (const auto& error : exceptionList_) {
            if (std::find(normalList_.begin(), normalList_.end(), error) != normalList_.end()) {
                return exceptionType(1);
            }
        }
    }

    return  exceptionType(0);
}

void RobotException::navigationResultCallback(const kybot_msg::SystemEvent::ConstPtr &result)
{
    navigationType_ = result->operationType;
    navigationResultNotify_ = result->results;
    kybot_msg::StateNotify navigationstate;
    if (navigationResultNotify_ == "find_obs")
    {
        navigationstate.exceptionInfo = "find_obs";
        AddErrorToErrorlist(navigationstate.exceptionInfo);
    }
}

void RobotException::exceptionNotifyCallback(const kybot_msg::StateNotify::ConstPtr &msg)
{
    AddErrorToErrorlist(msg->exceptionInfo);
}
void RobotException::locationdriftCallback(const std_msgs::Bool::ConstPtr &msg)
{
    locationdrift_ = msg->data;
}
bool RobotException::exceptionNotifySrvCallback(kybot_msg::exceptionNotifySrv::Request &req, kybot_msg::exceptionNotifySrv::Response &res)
{
    res.navigationType = navigationType_;
    res.navigationResultNotify = navigationResultNotify_;
    if (lastList_.size() == 0)
    {
        res.results.clear();
    }
    else
    {
        res.results = lastList_; 
    }
    lastList_.clear();
    navigationResultNotify_ = "";
    return true;
}
bool RobotException::stopLocationSrvcallback(kybot_msg::stopLocationSrv::Request &req, kybot_msg::stopLocationSrv::Response &res)
{
   
    if(req.stop)
    {
        stopLocation_ = true;
        res.failReason = "success";
    }
    else
    {
        stopLocation_ = false;
        res.failReason = "success" ;
    }
    LOG(INFO)<< "stopLocation_" <<stopLocation_;
    return true;
}
void RobotException::nodecheck()
{
    kybot_msg::StateNotify Nodestate;
    ros::V_string node_infos;
    ros::master::getNodes(node_infos);
    int node_count = 0;

#define CHECKNODEEXISTS(node, log)                                       \
    {                                                                    \
        auto it = std::find(node_infos.begin(), node_infos.end(), node); \
        if (it != node_infos.end())                                      \
        {                                                                \
        }                                                                \
        else                                                             \
        {                                                                \
            ROS_INFO(#log);                                              \
            node_count++;                                                \
        }                                                                \
    }


    if(checkFrontLaser_)
    CHECKNODEEXISTS(frontLaserNode_, frontLaserNode_ + "error\n")
    if(checkBackLaser_)
    CHECKNODEEXISTS(backLaserNode_, backLaserNode_ + "error\n")
    if(checkRightLaser_)
    CHECKNODEEXISTS(rightLaserNode_, rightLaserNode_ + "error\n")
    if(checkFrontCamera_)
    CHECKNODEEXISTS(frontCameraNode_, frontCameraNode_ + "error\n")
    if(checkBackCamera_)
    CHECKNODEEXISTS(backCameraNode_, backCameraNode_ + "error\n")
    if(checkOdom_)
    CHECKNODEEXISTS(odomNode_, odomNode_ + "error\n")
    if(checkImu_)
    CHECKNODEEXISTS(ImuNode_,ImuNode_ + "error\n")
    if(checkLocationSoft_)
    {
        CHECKNODEEXISTS(locationSoftNode_, locationSoftNode_ + "error\n")
        CHECKNODEEXISTS(locationSoftServerNode_, locationSoftServerNode_ + "error\n")
    }
    CHECKNODEEXISTS(interfaceNode_, interfaceNode_ + "error\n")
    CHECKNODEEXISTS(systemNode_, systemNode_ + "error\n")
    CHECKNODEEXISTS(navigationNode_, navigationNode_ + "/mcu_node error\n")
    CHECKNODEEXISTS(autoChargeNode_,autoChargeNode_ + "error\n")
    CHECKNODEEXISTS(perceptionNode_, perceptionNode_ + "error\n")
    CHECKNODEEXISTS(mapNode_, mapNode_ + "error\n")
    CHECKNODEEXISTS(accuratePositionNode_, accuratePositionNode_ + "error\n")
    CHECKNODEEXISTS(writeBagNode_, writeBagNode_ + "error\n")
    CHECKNODEEXISTS(mcuNode_, mcuNode_ + "error\n")
    CHECKNODEEXISTS(joyNode_, joyNode_ + "error\n")
    CHECKNODEEXISTS(joyControlNode_, joyControlNode_ + "error\n")
    CHECKNODEEXISTS(autoDeleteNode_, autoDeleteNode_ + "error\n")
    if (node_count > 0)
    {
        Nodestate.exceptionInfo = "nodeError";
        AddErrorToErrorlist(Nodestate.exceptionInfo);
    }
}

void RobotException::locationsoftCallback(const bosch_locator_bridge::ClientControlModeConstPtr &msg)
{
    Location_msg = *msg;
    location_recv_ = true;
}
void RobotException::locationposeCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
{
    location_nodata = false;

}
void RobotException::locationcheck()
{
    kybot_msg::StateNotify Locationstate;
    if (location_recv_&& Location_msg.visual_recording_state == 1 && Location_msg.map_state == 1 && Location_msg.recording_state == 1 && Location_msg.localization_state != 2 &&
        Location_msg.recording_state == 1 && Location_msg.alignment_state == 1 && Location_msg.mask_state == 1 && Location_msg.expandmap_state != 2 && !stopLocation_)
    {
        Locationstate.exceptionInfo = "locationError";
        LOG(INFO) << "location error1";
        AddErrorToErrorlist(Locationstate.exceptionInfo);
    }
    if (location_recv_&&(Location_msg.visual_recording_state == 4 || Location_msg.map_state == 4 || Location_msg.recording_state == 4 || Location_msg.localization_state == 4 ||
        Location_msg.recording_state == 4 || Location_msg.alignment_state == 4 || Location_msg.mask_state == 4 || Location_msg.expandmap_state == 4) && !stopLocation_)
    {
        Locationstate.exceptionInfo = "locationError";
        LOG(INFO)<<"location error2";
        AddErrorToErrorlist(Locationstate.exceptionInfo);
    }
}

bool RobotException::navigation_tostop()
{

        kybot_msg::TargetPoseNavigationControlSrv msg;
        std::string failReason_;
        msg.request.command = "stop";
        const std::string navigation_stop_srv = "/system_controller_node/target_pose_navigation_control_srv";
        if (ros::service::exists(navigation_stop_srv, false) && ros::service::call(navigation_stop_srv, msg))
        {
            if (!msg.response.success)
            {
                LOG(INFO) <<"target pose navigaiton control failure";
            }
            else
            {
                failReason_ = msg.response.failReason;
                LOG(INFO)<<"failReason_"<<failReason_.c_str();
                 return true;
            }
        }
        else
        {
            LOG(INFO)<<"call target pose navigation control service failed";
            failReason_ = "unknown";
            return false;
        }
}

void RobotException::HardwareCheck()
{
    // reset false after check
    for (auto iter = hardware_check_map_.begin(); iter != hardware_check_map_.end(); ++iter) {
        if (iter->second == false) {
            AddErrorToErrorlist(iter->first);
        }
        iter->second = false;
    }
}

void RobotException::CommunicationCheck()
{

}

void RobotException::BaseInfoCallback(const kybot_msg::BaseInfoConstPtr &msg)
{
    char error_code[255] = {0};
    std::vector<std::string> error_list;
    if (false == msg->commuValid)
        error_list.push_back("mcuCommuError");
    if (true == msg->estop)
        error_list.push_back("estop detected");
    if (0 != msg->mcuErrorCode) {
        sprintf(error_code, "mcuError:%d", msg->mcuErrorCode);
        error_list.push_back(error_code);
    }
    if (1 == msg->motorEnabled)
        error_list.push_back("motorEnabled");
    if (true == msg->drop)
        error_list.push_back("dropSensorError");

    if (error_list.size() > 0) {
        for (auto error: error_list){
            AddErrorToErrorlist(error);
        }
    }
}
void RobotException::McusendCallback(const std_msgs::String::ConstPtr &msg)
{
   mcucheckList_.push_back(msg->data);
}
void RobotException::McuerrorCallback(const std_msgs::String::ConstPtr &msg)
{
    mcucheckList_.push_back(msg->data);
}
void RobotException::AddErrorToErrorlist(const std::string& error)
{
    if (std::find(exceptionList_.begin(), exceptionList_.end(), error) == exceptionList_.end())
            exceptionList_.push_back(error);
}
